#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include<string>
#include "INIReader.h"
#include "ini.h"
#include"rectifyFisheye.hpp"
#include"opencv2/core/core.hpp"

#include"System.h"
//#include"PangolinViewer.h"
#include <unistd.h>
using namespace std;
using namespace cv;
using namespace xyVision;

void LoadImages(const string& strSequence, vector<string>& vstrImageFilenames,
				vector<double> &vTimestamps, vector<vector<double> >& gpses);

int main()
{
    RectifyFisheye rectify("config_savemap_3.ini");
    INIReader reader("config_savemap_3.ini");
    string imgPathTxt, voc_file, settings_file, map_save_file,
        trajectoryTxt, keyframeTrajectoryTxt;
    const char* section = "SaveMapData";
    const char* key = "imgPathTxt";
    imgPathTxt = reader.Get(section, key, " ");
    key = "voc_file";
    voc_file = reader.Get(section, key, " ");
    key = "settings_file";
    settings_file = reader.Get(section, key, " ");
    key = "map_save_file";
    map_save_file = reader.Get(section, key, " ");
    key = "trajectoryTxt";
    trajectoryTxt = reader.Get(section, key, " ");
    key = "keyframeTrajectoryTxt";
    keyframeTrajectoryTxt = reader.Get(section, key, " ");
    

	//const string voc_file = "ORBvoc.bin";
	//const string settings_file = "config_3.yaml";
	//const string map_save_file = "gc2_xu4.map.bin";
	string strSequence(imgPathTxt);
	vector<string> vstrImageFilenames;
	vector<double> vTimestamps;
    vector<vector<double> > gpses;
	LoadImages(strSequence, vstrImageFilenames, vTimestamps, gpses);
	int nImages = (int)vstrImageFilenames.size();

	//ORB_SLAM2::PangolinViewer viewer(settings_file);
	ORB_SLAM2::System SLAM(voc_file, settings_file, ORB_SLAM2::System::MONOCULAR, false);

    // Vector for tracking time statistics
    vector<float> vTimesTrack;
    vTimesTrack.resize(nImages);

    cout << endl << "-------" << endl;
    cout << "Start processing sequence ..." << endl;
    cout << "Images in the sequence: " << nImages << endl << endl;

    // Main loop
    cv::Mat im, imFisheye;

    for(int ni=0; ni<nImages; ni++)
    {
        // Read image from file
        imFisheye = cv::imread(vstrImageFilenames[ni],CV_LOAD_IMAGE_UNCHANGED);
        // rectify image
        rectify.rectifyImg(imFisheye, im);
        //im = cv::imread(image_name,CV_LOAD_IMAGE_UNCHANGED);
        double tframe = vTimestamps[ni];
        vector<double> gps = gpses[ni];
		cerr << ni << endl;
        if(im.empty())
        {
            cerr << endl << "Failed to load image at: " << vstrImageFilenames[ni] << endl;
            return 1;
        }

#ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
#endif

        // Pass the image to the SLAM system
        SLAM.TrackMonocular(im,tframe,gps);

#ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
#endif

        double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();

        vTimesTrack[ni]=ttrack;

        // Wait to load the next frame
        double T=0;
        if(ni<nImages-1)
            T = vTimestamps[ni+1]-tframe;
        else if(ni>0)
            T = tframe-vTimestamps[ni-1];

        if(ttrack<T)
            usleep((T-ttrack)*1e6);
    }
	sleep(5);
	SLAM.Shutdown();
	SLAM.SaveMap(map_save_file);

	// Tracking time statistics
    sort(vTimesTrack.begin(),vTimesTrack.end());
    float totaltime = 0;
    for(int ni=0; ni<nImages; ni++)
    {
        totaltime+=vTimesTrack[ni];
    }
    cout << "-------" << endl << endl;
    cout << "median tracking time: " << vTimesTrack[nImages/2] << endl;
    cout << "mean tracking time: " << totaltime/nImages << endl;
    SLAM.SaveTrajectoryTUM(trajectoryTxt);
    SLAM.SaveKeyFrameTrajectoryTUM(keyframeTrajectoryTxt);
    return 0;

}

void LoadImages(const string& strSequence, vector<string>& vstrImageFilenames,
                vector<double> &vTimestamps, vector<vector<double> >& gpses)
{
	vstrImageFilenames.resize(0);
	vTimestamps.resize(0);
    gpses.resize(0);
	double time = 0.0;
	ifstream fs(strSequence.c_str());
	if(!fs.is_open())
	{
		cout << "Error opening file" <<endl;
	}
	char buffer[512];
    char file_name[512];
    double lat, lon;

	while(!fs.eof())
	{
		fs.getline(buffer, 512);
        sscanf(buffer, "%s,%lf,%lf", file_name, &lat, &lon);
		vstrImageFilenames.push_back(string(file_name));
		vTimestamps.push_back(time);
        vector<double> gps;
        gps.push_back(lat);
        gps.push_back(lon);
        gpses.push_back(gps)
		time = time + 1.0;
	}
}
